#include <memory>
#include <chrono>
#include <vector>
#include <algorithm>
#include <cmath>

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "fruit_nav/action/navigate_to_pose.hpp"
#include "arm_control/action/arm.hpp"
#include "qr_recognition/action/recognize_qr.hpp"
#include "voice_planner/action/voice_planner.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "std_msgs/msg/int32.hpp"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "task_planner/waypoints_config.hpp"
#include "ament_index_cpp/get_package_share_directory.hpp"

using namespace std::chrono_literals;

class TaskPlanner : public rclcpp::Node
{
public:
  using NavigateToPose = fruit_nav::action::NavigateToPose;
  using GoalHandleNavigateToPose = rclcpp_action::ClientGoalHandle<NavigateToPose>;
  using ARM = arm_control::action::ARM;
  using GoalHandleARM = rclcpp_action::ClientGoalHandle<ARM>;
  using RecognizeQR = qr_recognition::action::RecognizeQR;
  using GoalHandleRecognizeQR = rclcpp_action::ClientGoalHandle<RecognizeQR>;
  using VoicePlanner = voice_planner::action::VoicePlanner;
  using GoalHandleVoicePlanner = rclcpp_action::ClientGoalHandle<VoicePlanner>;

  // 使用配置文件中的WayPoint结构体
  using WayPoint = task_planner::WayPoint;

  TaskPlanner() : Node("task_planner_node"), current_goal_index_(0)
  {
    RCLCPP_INFO(this->get_logger(), "任务规划节点已启动");
    
    // 加载配置文件
    loadWaypointsConfig();

    // 创建action客户端
    this->action_client_ = rclcpp_action::create_client<NavigateToPose>(
      this, "/navigate_to_pose");
    
    // 创建ARM action客户端
    this->arm_action_client_ = rclcpp_action::create_client<ARM>(
      this, "/ARM");
    
    // 创建RecognizeQR action客户端
    this->qr_action_client_ = rclcpp_action::create_client<RecognizeQR>(
      this, "/recognize_qr");
    
    // 创建VoicePlanner action客户端
    this->voice_planner_client_ = rclcpp_action::create_client<VoicePlanner>(
      this, "/voice_planner");

    // 创建detect_part发布器
    this->detect_part_publisher_ = this->create_publisher<std_msgs::msg::Int32>(
      "/detect_part", 10);

    // 启动时发布1到/detect_part（连续发布3次）
    auto msg = std_msgs::msg::Int32();
    msg.data = 1;
    for (int i = 0; i < 3; ++i)
    {
      detect_part_publisher_->publish(msg);
      std::this_thread::sleep_for(100ms);
    }
    RCLCPP_INFO(this->get_logger(), "已发布detect_part = 1（启动，连续3次）");

    // 目标点将从配置文件加载
    // waypoints_ 将在 loadWaypointsConfig() 中初始化

    // 等待action服务器可用
    RCLCPP_INFO(this->get_logger(), "等待action服务器 /navigate_to_pose ...");
    if (!action_client_->wait_for_action_server(10s))
    {
      RCLCPP_ERROR(this->get_logger(), "Action服务器不可用，请确保fruit_nav节点已启动");
      rclcpp::shutdown();
      return;
    }

    RCLCPP_INFO(this->get_logger(), "Action服务器已连接");

    // 开始前先执行voice_planner动作1，并等待15秒
    execute_initial_voice_planner();
  }

private:
  rclcpp_action::Client<NavigateToPose>::SharedPtr action_client_;
  rclcpp_action::Client<ARM>::SharedPtr arm_action_client_;
  rclcpp_action::Client<RecognizeQR>::SharedPtr qr_action_client_;
  rclcpp_action::Client<VoicePlanner>::SharedPtr voice_planner_client_;
  rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr detect_part_publisher_;
  std::vector<WayPoint> waypoints_;
  size_t current_goal_index_;
  
  // 配置加载器
  task_planner::WaypointsConfig config_;
  
  // 存储QR识别结果（type1 - B区）
  std::vector<int32_t> qr_type1_fruit_array_;
  std::vector<int32_t> qr_type1_number_array_;
  
  // 存储QR识别结果（type2 - C区）
  std::vector<int32_t> qr_type2_fruit_array_;
  std::vector<int32_t> qr_type2_number_array_;
  
  // 追踪当前正在执行的数字目标索引（从0开始）
  size_t current_number_index_ = 0;

  // 加载配置文件
  void loadWaypointsConfig()
  {
    try
    {
      // 获取包的共享目录
      std::string package_share_directory = ament_index_cpp::get_package_share_directory("task_planner");
      std::string config_file_path = package_share_directory + "/config/waypoints_config.yaml";
      
      RCLCPP_INFO(this->get_logger(), "加载配置文件: %s", config_file_path.c_str());
      
      if (config_.loadFromFile(config_file_path))
      {
        // 从配置文件加载初始目标点
        waypoints_ = config_.getInitialWaypoints();
        RCLCPP_INFO(this->get_logger(), "从配置文件加载了 %zu 个目标点", waypoints_.size());
      }
      else
      {
        RCLCPP_ERROR(this->get_logger(), "配置文件加载失败，使用默认配置");
        loadDefaultWaypoints();
      }
    }
    catch (const std::exception& e)
    {
      RCLCPP_ERROR(this->get_logger(), "加载配置文件异常: %s，使用默认配置", e.what());
      loadDefaultWaypoints();
    }
  }
  
  // 加载默认配置（作为备用）
  void loadDefaultWaypoints()
  {
    waypoints_ = {
      {0.71, 0.0, 0.0, 0.0},       // 目标点1 (0度)
      {1.21, 0.0, 0.0, 0.0},       // 目标点2 (0度)
      {1.71, 0.0, 0.0, 0.0},       // 目标点3 (0度)
      {2.21, 0.0, 0.0, 0.0},       // 目标点4 (0度)
      {2.35, 0.0, 0.0, 0.0},       // 目标点5: A区到B区转弯启动点 (0度)
      {2.5, -0.3, 0.0, -45.0},     // 目标点6: A区到B区45度转弯目标点 (-45度)
      {2.5, -0.55, 0.0, -90.0},    // 目标点7: B区扫码 (-90度)
      {2.5, -0.9, 0.0, -90.0},     // 目标点8: B区入口 (-90度)
      {2.0, -1.0, 0.0, 180.0},     // 目标点9 (180度)
      {1.70, -1.0, 0.0, 180.0},    // 目标点10 (180度)
      {1.53, -1.0, 0.0, 180.0},    // 目标点11 (180度)
      {1.25, -1.0, 0.0, 180.0},    // 目标点12 (180度)
      {1.03, -1.0, 0.0, 180.0},    // 目标点13 (180度)
      {0.75, -1.0, 0.0, 180.0},    // 目标点14 (180度)
      {0.20, -1.0, 0.0, 180.0},    // 目标点15 (180度)
      {0.15, -1.0, 0.0, 180.0},    // 目标点16 (180度)
      {0.05, -1.3, 0.0, -135.0},   // 目标点17 (-135度)
      {0.1, -2.0, 0.0, -90.0}      // 目标点18 (-90度)
    };
    RCLCPP_WARN(this->get_logger(), "使用默认配置，共 %zu 个目标点", waypoints_.size());
  }

  // 将yaw角（角度制）转换为四元数
  geometry_msgs::msg::Quaternion yaw_to_quaternion(double yaw_degree)
  {
    // 将角度转换为弧度
    double yaw_rad = yaw_degree * M_PI / 180.0;
    tf2::Quaternion q;
    q.setRPY(0, 0, yaw_rad);
    return tf2::toMsg(q);
  }

  // 根据数字（1-12）获取对应的位置和姿态
  // 使用配置文件中的数据
  WayPoint get_position_for_number(int32_t number)
  {
    return config_.getNumberPosition(number);
  }

  // 判断数字点是否在路径段上
  // segment: 0=A→AB中点, 1=AB中点→B, 2=B→C, 3=C→CD中点, 4=CD中点→D, 5=D→A
  // 仅在段0,1(底部)和段3,4(顶部)添加数字目标
  bool is_number_on_segment(int32_t num, int segment)
  {
    WayPoint point = get_position_for_number(num);
    
    const double top_y = config_.getTopY();        // 从配置获取顶部y坐标
    const double bottom_y = config_.getBottomY();  // 从配置获取底部y坐标
    const double tolerance = config_.getTolerance(); // 从配置获取容差
    
    switch(segment)
    {
      case 0: // A→AB中点：底部左半段
        return (std::abs(point.y - bottom_y) < tolerance);
        
      case 1: // AB中点→B：底部右半段
        return (std::abs(point.y - bottom_y) < tolerance);
        
      case 2: // B→C：右侧过渡段，不添加数字目标
        return false;
        
      case 3: // C→CD中点：顶部右半段
        return (std::abs(point.y - top_y) < tolerance);
        
      case 4: // CD中点→D：顶部左半段
        return (std::abs(point.y - top_y) < tolerance);
        
      case 5: // D→A：左侧过渡段，不添加数字目标
        return false;
        
      default:
        return false;
    }
  }
  
  // 在路径段上收集所有需要访问的数字点，并按路径方向排序
  std::vector<int32_t> get_numbers_on_segment(int segment, const std::vector<int32_t>& remaining_numbers)
  {
    std::vector<std::pair<double, int32_t>> numbers_with_position;
    
    for (int32_t num : remaining_numbers)
    {
      if (is_number_on_segment(num, segment))
      {
        WayPoint point = get_position_for_number(num);
        double sort_key = 0.0;
        
        switch(segment)
        {
          case 0: sort_key = point.x; break;   // A→AB中点（底部左）：x递增（从左到右）
          case 1: sort_key = point.x; break;   // AB中点→B（底部右）：x递增（从左到右）
          case 2: sort_key = -point.y; break;  // B→C（右侧）：y递增（从下到上）
          case 3: sort_key = -point.x; break;  // C→CD中点（顶部右）：x递减（从右到左）
          case 4: sort_key = -point.x; break;  // CD中点→D（顶部左）：x递减（从右到左）
          case 5: sort_key = point.y; break;   // D→A（左侧）：y递减（从上到下）
        }
        
        numbers_with_position.push_back({sort_key, num});
      }
    }
    
    // 按排序键排序
    std::sort(numbers_with_position.begin(), numbers_with_position.end());
    
    std::vector<int32_t> result;
    for (const auto& pair : numbers_with_position)
    {
      result.push_back(pair.second);
    }
    
    return result;
  }

  // 根据数字数组生成按路径规划的目标点序列（循环模式）
  void generate_waypoints_from_numbers(const std::vector<int32_t>& numbers)
  {
    RCLCPP_INFO(this->get_logger(), "======================================");

    RCLCPP_INFO(this->get_logger(), "根据数字数组生成目标点序列（A→B→C→D循环模式 - 优化版）...");
    
    // 打印接收到的数字数组
    std::string numbers_str = "数字数组: [";
    for (size_t i = 0; i < numbers.size(); ++i)
    {
      numbers_str += std::to_string(numbers[i]);
      if (i < numbers.size() - 1)
        numbers_str += ", ";
    }
    numbers_str += "]";
    RCLCPP_INFO(this->get_logger(), "%s", numbers_str.c_str());
    
    // 清空当前的目标点列表，保留前18个已完成的目标点
    waypoints_.resize(18);
    
    // 从配置文件获取C区过渡点
    std::vector<WayPoint> corner_points = config_.getCZoneCornerPointsArray();
    
    // 路径段名称（用于日志）
    const char* segment_names[6] = {
      "A→AB中点(底部左)", "AB中点→B(底部右)", "B→C(右侧)", 
      "C→CD中点(顶部右)", "CD中点→D(顶部左)", "D→A(左侧)"
    };
    
    size_t next_target_index = 0;  // 下一个要访问的数字在原数组中的索引
    
    RCLCPP_INFO(this->get_logger(), "开始循环路径，一次循环可访问多个数字点...");
    
    // 循环直到所有数字都被访问
    int max_loops = 100;  // 防止无限循环
    int loop_count = 0;
    
    while (next_target_index < numbers.size() && loop_count < max_loops)
    {
      loop_count++;
      RCLCPP_INFO(this->get_logger(), "--- 循环 #%d ---", loop_count);
      
      // 遍历6条路径段
      for (int segment = 0; segment < 6; segment++)
      {
        // 收集当前路径段上的所有连续数字目标
        std::vector<int32_t> targets_on_segment;
        size_t temp_index = next_target_index;
        
        // 从next_target_index开始，收集所有在当前路径段上的连续数字
        while (temp_index < numbers.size() && is_number_on_segment(numbers[temp_index], segment))
        {
          targets_on_segment.push_back(numbers[temp_index]);
          temp_index++;
        }
        
        if (!targets_on_segment.empty())
        {
          // 有目标在此段，添加过渡点
          const char* point_names[6] = {"A", "AB中点", "B", "C", "CD中点", "D"};
          waypoints_.push_back(corner_points[segment]);
          RCLCPP_INFO(this->get_logger(), "  过渡点%s: (%.2f, %.2f, yaw=%.2f)", 
                      point_names[segment], corner_points[segment].x, corner_points[segment].y, corner_points[segment].yaw);
          
          // 按路径方向对这些目标排序
          std::vector<std::pair<double, int32_t>> sorted_targets;
          for (int32_t num : targets_on_segment)
          {
            WayPoint point = get_position_for_number(num);
            double sort_key = 0.0;
            
            switch(segment)
            {
              case 0: sort_key = point.x; break;   // A→AB中点：x递增
              case 1: sort_key = point.x; break;   // AB中点→B：x递增
              case 2: sort_key = -point.y; break;  // B→C：y递增
              case 3: sort_key = -point.x; break;  // C→CD中点：x递减
              case 4: sort_key = -point.x; break;  // CD中点→D：x递减
              case 5: sort_key = point.y; break;   // D→A：y递减
            }
            
            sorted_targets.push_back({sort_key, num});
          }
          
          // 按排序键排序
          std::sort(sorted_targets.begin(), sorted_targets.end());
          
          // 依次添加所有排序后的目标点
          for (const auto& pair : sorted_targets)
          {
            int32_t num = pair.second;
            WayPoint target_point = get_position_for_number(num);
            target_point.number_id = num;  // 设置数字点编号
            waypoints_.push_back(target_point);
            RCLCPP_INFO(this->get_logger(), "  ✓ 访问数字%d: (%.2f, %.2f, yaw=%.2f) [%s]", 
                        num, target_point.x, target_point.y, target_point.yaw, segment_names[segment]);
          }
          
          // 更新next_target_index
          next_target_index = temp_index;
        }
        else
        {
          // 没有目标在此段，只添加过渡点
          const char* point_names[6] = {"A", "AB中点", "B", "C", "CD中点", "D"};
          waypoints_.push_back(corner_points[segment]);
          RCLCPP_INFO(this->get_logger(), "  过渡点%s: (%.2f, %.2f, yaw=%.2f)", 
                      point_names[segment], corner_points[segment].x, corner_points[segment].y, corner_points[segment].yaw);
        }
      }
      
      // 如果所有数字都已访问，退出循环
      if (next_target_index >= numbers.size())
      {
        RCLCPP_INFO(this->get_logger(), "所有数字点已访问完成！");
        break;
      }
    }
    
    // 添加最终目标点（返回到A点）
    RCLCPP_INFO(this->get_logger(), "添加最终目标点A: (0.1, -2.67, 0.0)");
    waypoints_.push_back(WayPoint(0.1, -2.67, 0.0, 0.0));
    
    RCLCPP_INFO(this->get_logger(), "总共生成 %zu 个目标点", waypoints_.size() - 17);
    RCLCPP_INFO(this->get_logger(), "======================================");
  }

  // 发送下一个目标点
  void send_next_goal()
  {
    if (current_goal_index_ >= waypoints_.size())
    {
      RCLCPP_INFO(this->get_logger(), "========================================");
      RCLCPP_INFO(this->get_logger(), "所有目标点已完成！任务规划执行结束。");
      RCLCPP_INFO(this->get_logger(), "========================================");
      return;
    }

    auto& waypoint = waypoints_[current_goal_index_];

    RCLCPP_INFO(this->get_logger(), "========================================");
    RCLCPP_INFO(this->get_logger(), "发送目标点 %zu/%zu", 
                current_goal_index_ + 1, waypoints_.size());
    RCLCPP_INFO(this->get_logger(), "  位置: x=%.2f, y=%.2f", waypoint.x, waypoint.y);
    RCLCPP_INFO(this->get_logger(), "  姿态: yaw=%.2f rad (%.1f度)", 
                waypoint.yaw, waypoint.yaw * 180.0 / M_PI);
    RCLCPP_INFO(this->get_logger(), "========================================");

    // 创建目标消息
    auto goal_msg = NavigateToPose::Goal();
    goal_msg.target_pose.header.frame_id = "map";
    goal_msg.target_pose.header.stamp = this->now();
    goal_msg.target_pose.pose.position.x = waypoint.x;
    goal_msg.target_pose.pose.position.y = waypoint.y;
    goal_msg.target_pose.pose.position.z = 0.0;
    goal_msg.target_pose.pose.orientation = yaw_to_quaternion(waypoint.yaw);

    // 配置发送选项
    auto send_goal_options = rclcpp_action::Client<NavigateToPose>::SendGoalOptions();
    
    // 设置目标响应回调
    send_goal_options.goal_response_callback =
      std::bind(&TaskPlanner::goal_response_callback, this, std::placeholders::_1);
    
    // 设置反馈回调
    send_goal_options.feedback_callback =
      std::bind(&TaskPlanner::feedback_callback, this, std::placeholders::_1, std::placeholders::_2);
    
    // 设置结果回调
    send_goal_options.result_callback =
      std::bind(&TaskPlanner::result_callback, this, std::placeholders::_1);

    // 发送目标
    action_client_->async_send_goal(goal_msg, send_goal_options);
  }

  // 目标响应回调
  void goal_response_callback(const GoalHandleNavigateToPose::SharedPtr & goal_handle)
  {
    if (!goal_handle)
    {
      RCLCPP_ERROR(this->get_logger(), "目标被服务器拒绝");
      current_goal_index_++;
      send_next_goal();
    }
    // 目标被接受时不输出日志，减少冗余信息
  }

  // 反馈回调
  void feedback_callback(
    GoalHandleNavigateToPose::SharedPtr,
    const std::shared_ptr<const NavigateToPose::Feedback> feedback)
  {
    // 减少日志输出，仅在需要调试时取消注释
    // RCLCPP_INFO(this->get_logger(), 
    //             "当前位置: x=%.2f, y=%.2f | 剩余距离: %.2f m", 
    //             feedback->current_pose.pose.position.x,
    //             feedback->current_pose.pose.position.y,
    //             feedback->distance_remaining);
  }

  // 结果回调
  void result_callback(const GoalHandleNavigateToPose::WrappedResult & result)
  {
    switch (result.code)
    {
      case rclcpp_action::ResultCode::SUCCEEDED:
        RCLCPP_INFO(this->get_logger(), "✓ 目标点 %zu 完成: %s", 
                    current_goal_index_ + 1, 
                    result.result->message.c_str());
        break;
      case rclcpp_action::ResultCode::ABORTED:
        RCLCPP_ERROR(this->get_logger(), "✗ 目标点 %zu 中止: %s", 
                     current_goal_index_ + 1,
                     result.result->message.c_str());
        break;
      case rclcpp_action::ResultCode::CANCELED:
        RCLCPP_WARN(this->get_logger(), "✗ 目标点 %zu 被取消", 
                    current_goal_index_ + 1);
        break;
      default:
        RCLCPP_ERROR(this->get_logger(), "✗ 目标点 %zu 未知结果", 
                     current_goal_index_ + 1);
        break;
    }

    // 如果是目标点1、2、3、4（索引0、1、2、3），并且成功到达，则执行机械臂动作
    if (result.code == rclcpp_action::ResultCode::SUCCEEDED && 
        current_goal_index_ >= 0 && current_goal_index_ <= 3)
    {
      RCLCPP_INFO(this->get_logger(), "到达目标点 %zu，准备执行机械臂动作...", 
                  current_goal_index_ + 1);
      std::this_thread::sleep_for(1s);
      
      // 所有目标点都执行动作1-2-3序列（目标点4会在动作3完成后继续执行动作4）
      execute_arm_actions();
    }
    // 如果是目标点7（索引6，B区扫码点），并且成功到达，则执行动作5、二维码识别
    else if (result.code == rclcpp_action::ResultCode::SUCCEEDED && 
             current_goal_index_ == 6)
    {
      RCLCPP_INFO(this->get_logger(), "到达目标点 %zu（B区扫码点），准备执行机械臂动作5、二维码识别...", 
                  current_goal_index_ + 1);
      
      // 到达目标点7后，发布2到/detect_part（连续发布3次）
      auto msg = std_msgs::msg::Int32();
      msg.data = 2;
      for (int i = 0; i < 3; ++i)
      {
        detect_part_publisher_->publish(msg);
        std::this_thread::sleep_for(100ms);
      }
      RCLCPP_INFO(this->get_logger(), "已发布detect_part = 2（到达目标点7，连续3次）");
      
      std::this_thread::sleep_for(1s);
      execute_arm_action_type5_qr_type1();
    }
    // 如果是目标点9、11、13（索引8、10、12），并且成功到达，则执行动作9
    else if (result.code == rclcpp_action::ResultCode::SUCCEEDED && 
             (current_goal_index_ == 8 || current_goal_index_ == 10 || current_goal_index_ == 12))
    {
      RCLCPP_INFO(this->get_logger(), "到达目标点 %zu，准备执行机械臂动作9...", 
                  current_goal_index_ + 1);
      std::this_thread::sleep_for(1s);
      execute_arm_action_9_then_navigate();
    }
    // 如果是目标点10、12、14、15（索引9、11、13、14），并且成功到达，则执行动作10
    else if (result.code == rclcpp_action::ResultCode::SUCCEEDED && 
             (current_goal_index_ == 9 || current_goal_index_ == 11 || current_goal_index_ == 13 || current_goal_index_ == 14))
    {
      RCLCPP_INFO(this->get_logger(), "到达目标点 %zu，准备执行机械臂动作10...", 
                  current_goal_index_ + 1);
      std::this_thread::sleep_for(1s);
      execute_arm_action_10_then_navigate();
    }
    // 如果是目标点16（索引15，B区到C区转弯启动点），并且成功到达，则执行动作4
    else if (result.code == rclcpp_action::ResultCode::SUCCEEDED && 
             current_goal_index_ == 15)
    {
      RCLCPP_INFO(this->get_logger(), "到达目标点 %zu（B区到C区转弯启动点），准备执行机械臂动作4...", 
                  current_goal_index_ + 1);
      std::this_thread::sleep_for(1s);
      execute_single_arm_action_4();
    }
    // 如果是目标点18（索引17，C区入口），并且成功到达，则先执行动作5再扫码
    else if (result.code == rclcpp_action::ResultCode::SUCCEEDED && 
             current_goal_index_ == 17)
    {
      RCLCPP_INFO(this->get_logger(), "到达目标点 %zu（C区入口），准备执行机械臂动作5，然后扫码...", 
                  current_goal_index_ + 1);
      
      // 到达目标点18后，发布3到/detect_part（连续发布3次）
      auto msg = std_msgs::msg::Int32();
      msg.data = 3;
      for (int i = 0; i < 3; ++i)
      {
        detect_part_publisher_->publish(msg);
        std::this_thread::sleep_for(100ms);
      }
      RCLCPP_INFO(this->get_logger(), "已发布detect_part = 3（到达目标点18，连续3次）");
      
      std::this_thread::sleep_for(1s);
      // 先执行动作5，完成后再执行二维码识别type2
      execute_arm_action_type5_qr_type2();
    }
    // 如果是C区数字点（索引>=18且number_id不为0），并且成功到达，则执行机械臂动作
    else if (result.code == rclcpp_action::ResultCode::SUCCEEDED && 
             current_goal_index_ >= 18 && 
             waypoints_[current_goal_index_].number_id != 0)
    {
      int32_t number_id = waypoints_[current_goal_index_].number_id;
      RCLCPP_INFO(this->get_logger(), "到达C区数字点 %d，准备执行机械臂动作...", number_id);
      std::this_thread::sleep_for(1s);
      
      // 根据数字点编号决定执行哪个动作
      // 数字点1,2,3,4,9,10,11,12执行动作8
      // 数字点5,6,7,8执行动作7
      if (number_id == 1 || number_id == 2 || number_id == 3 || number_id == 4 ||
          number_id == 9 || number_id == 10 || number_id == 11 || number_id == 12)
      {
        execute_arm_action_8_then_navigate();
      }
      else if (number_id == 5 || number_id == 6 || number_id == 7 || number_id == 8)
      {
        execute_arm_action_7_then_navigate();
      }
      else
      {
        // 其他数字点（不应该出现），直接继续导航
        RCLCPP_WARN(this->get_logger(), "未知的数字点编号 %d，跳过机械臂动作", number_id);
        current_goal_index_++;
        std::this_thread::sleep_for(2s);
        send_next_goal();
      }
    }
    else
    {
      // 继续发送下一个目标点
      current_goal_index_++;
      
      // 等待一小段时间后发送下一个目标
      if (current_goal_index_ < waypoints_.size())
      {
        // 如果是C区的目标点（索引>=18），使用2秒等待；否则使用1秒
        if (current_goal_index_ >= 18)
        {
          std::this_thread::sleep_for(2s);
        }
        else
        {
          std::this_thread::sleep_for(1s);
        }
        send_next_goal();
      }
      else
      {
        send_next_goal();  // 打印完成信息
      }
    }
  }

  // 执行ARM动作序列（动作1-2-3）
  void execute_arm_actions()
  {
    RCLCPP_INFO(this->get_logger(), "======================================");
    RCLCPP_INFO(this->get_logger(), "开始执行机械臂动作序列（动作1-2-3）...");
    RCLCPP_INFO(this->get_logger(), "======================================");
    
    // 等待ARM action服务器可用
    if (!arm_action_client_->wait_for_action_server(5s))
    {
      RCLCPP_ERROR(this->get_logger(), "ARM Action服务器不可用，跳过机械臂动作");
      // 继续下一个目标点
      current_goal_index_++;
      send_next_goal();
      return;
    }

    // 执行动作1
    execute_single_arm_action(1);
  }

  // 执行单个ARM动作
  void execute_single_arm_action(int action_type)
  {
    auto goal_msg = ARM::Goal();
    goal_msg.action_type = action_type;
    
    // 如果是动作2，需要设置left_type和right_type参数
    if (action_type == 2)
    {
      // 目标点1、2、3、4（索引0、1、2、3）使用类型0
      if (current_goal_index_ >= 0 && current_goal_index_ <= 3)
      {
        goal_msg.left_type = {0};   // 左侧抓取类型0的水果
        goal_msg.right_type = {0};  // 右侧抓取类型0的水果
        RCLCPP_INFO(this->get_logger(), "发送机械臂动作 %d（目标点%d，左侧类型: [0], 右侧类型: [0]）...", 
                    action_type, current_goal_index_ + 1);
      }
      else
      {
        // 其他目标点使用类型1、2、3
        goal_msg.left_type = {1, 2, 3};   // 左侧抓取类型1、2、3的水果
        goal_msg.right_type = {1, 2, 3};  // 右侧抓取类型1、2、3的水果
        RCLCPP_INFO(this->get_logger(), "发送机械臂动作 %d（左侧类型: [1,2,3], 右侧类型: [1,2,3]）...", action_type);
      }
    }
    else
    {
      RCLCPP_INFO(this->get_logger(), "发送机械臂动作 %d...", action_type);
    }

    auto send_goal_options = rclcpp_action::Client<ARM>::SendGoalOptions();
    
    // 设置结果回调
    send_goal_options.result_callback =
      [this, action_type](const GoalHandleARM::WrappedResult & result) {
        if (result.code == rclcpp_action::ResultCode::SUCCEEDED)
        {
          RCLCPP_INFO(this->get_logger(), "✓ 机械臂动作 %d 完成: %s", 
                      action_type, result.result->message.c_str());
          
          // 如果是动作1，继续执行动作2
          if (action_type == 1)
          {
            RCLCPP_INFO(this->get_logger(), "等待1秒后执行动作2...");
            std::this_thread::sleep_for(1s);
            execute_single_arm_action(2);
          }
          // 如果是动作2，继续执行动作3
          else if (action_type == 2)
          {
            RCLCPP_INFO(this->get_logger(), "等待1秒后执行动作3...");
            std::this_thread::sleep_for(1s);
            execute_single_arm_action(3);
          }
          // 如果是动作3，需要判断是否是目标点4
          else if (action_type == 3)
          {
            // 如果是目标点4（索引3），继续执行动作4
            if (current_goal_index_ == 3)
            {
              RCLCPP_INFO(this->get_logger(), "目标点4：动作3完成，等待1秒后执行动作4...");
              std::this_thread::sleep_for(1s);
              execute_single_arm_action(4);
            }
            else
            {
              // 其他目标点，动作3完成后继续导航
              RCLCPP_INFO(this->get_logger(), "机械臂动作完成，等待3秒后继续导航...");
              current_goal_index_++;
              std::this_thread::sleep_for(3s);
              send_next_goal();
            }
          }
          // 如果是动作4，继续下一个目标点
          else if (action_type == 4)
          {
            RCLCPP_INFO(this->get_logger(), "机械臂动作完成，等待3秒后继续导航...");
            current_goal_index_++;
            std::this_thread::sleep_for(3s);
            send_next_goal();
          }
        }
        else
        {
          RCLCPP_ERROR(this->get_logger(), "✗ 机械臂动作 %d 失败: %s", 
                       action_type, result.result->message.c_str());
          
          // 如果是目标点4（索引3），即使失败也要执行后续动作直到动作4
          if (current_goal_index_ == 3)
          {
            // 如果是动作1失败，继续执行动作2
            if (action_type == 1)
            {
              RCLCPP_WARN(this->get_logger(), "目标点4：动作1失败，但继续执行动作2...");
              std::this_thread::sleep_for(1s);
              execute_single_arm_action(2);
              return;
            }
            // 如果是动作2失败，直接跳到动作4
            else if (action_type == 2)
            {
              RCLCPP_WARN(this->get_logger(), "目标点4：动作2失败，直接执行动作4...");
              std::this_thread::sleep_for(1s);
              execute_single_arm_action(4);
              return;
            }
            // 如果是动作3失败，继续执行动作4
            else if (action_type == 3)
            {
              RCLCPP_WARN(this->get_logger(), "目标点4：动作3失败，但继续执行动作4...");
              std::this_thread::sleep_for(1s);
              execute_single_arm_action(4);
              return;
            }
            // 如果是动作4失败，才继续下一个目标点
            else if (action_type == 4)
            {
              RCLCPP_WARN(this->get_logger(), "目标点4：动作4失败，继续下一个目标点...");
              current_goal_index_++;
              send_next_goal();
              return;
            }
          }
          
          // 其他情况，失败后继续下一个目标点
          current_goal_index_++;
          send_next_goal();
        }
      };

    // 设置反馈回调（减少日志输出）
    send_goal_options.feedback_callback =
      [this, action_type](GoalHandleARM::SharedPtr,
                         const std::shared_ptr<const ARM::Feedback> feedback) {
        // RCLCPP_INFO(this->get_logger(), "动作 %d 进度: %.1f%% - %s", 
        //             action_type, feedback->progress_percentage, 
        //             feedback->current_status.c_str());
      };

    arm_action_client_->async_send_goal(goal_msg, send_goal_options);
  }

  // 执行单个ARM动作4，完成后继续导航
  void execute_single_arm_action_4()
  {
    RCLCPP_INFO(this->get_logger(), "======================================");
    RCLCPP_INFO(this->get_logger(), "开始执行机械臂动作4...");
    RCLCPP_INFO(this->get_logger(), "======================================");
    
    // 等待ARM action服务器可用
    if (!arm_action_client_->wait_for_action_server(5s))
    {
      RCLCPP_ERROR(this->get_logger(), "ARM Action服务器不可用，跳过机械臂动作");
      // 继续下一个目标点
      current_goal_index_++;
      send_next_goal();
      return;
    }

    auto goal_msg = ARM::Goal();
    goal_msg.action_type = 4;
    
    RCLCPP_INFO(this->get_logger(), "发送机械臂动作 4...");

    auto send_goal_options = rclcpp_action::Client<ARM>::SendGoalOptions();
    
    // 设置结果回调
    send_goal_options.result_callback =
      [this](const GoalHandleARM::WrappedResult & result) {
        if (result.code == rclcpp_action::ResultCode::SUCCEEDED)
        {
          RCLCPP_INFO(this->get_logger(), "✓ 机械臂动作 4 完成: %s", 
                      result.result->message.c_str());
          
          // 动作4完成后，继续导航
          RCLCPP_INFO(this->get_logger(), "机械臂动作完成，等待3秒后继续导航...");
          current_goal_index_++;
          std::this_thread::sleep_for(3s);
          send_next_goal();
        }
        else
        {
          RCLCPP_ERROR(this->get_logger(), "✗ 机械臂动作 4 失败: %s", 
                       result.result->message.c_str());
          // 失败也继续下一个目标点
          current_goal_index_++;
          send_next_goal();
        }
      };

    // 设置反馈回调（减少日志输出）
    send_goal_options.feedback_callback =
      [this](GoalHandleARM::SharedPtr,
             const std::shared_ptr<const ARM::Feedback> feedback) {
        // RCLCPP_INFO(this->get_logger(), "动作 4 进度: %.1f%% - %s", 
        //             feedback->progress_percentage, 
        //             feedback->current_status.c_str());
      };

    arm_action_client_->async_send_goal(goal_msg, send_goal_options);
  }

  // 执行ARM动作5，完成后执行二维码识别type1，然后继续导航
  void execute_arm_action_type5_qr_type1()
  {
    RCLCPP_INFO(this->get_logger(), "======================================");
    RCLCPP_INFO(this->get_logger(), "开始执行机械臂动作5...");
    RCLCPP_INFO(this->get_logger(), "======================================");
    
    // 等待ARM action服务器可用
    if (!arm_action_client_->wait_for_action_server(5s))
    {
      RCLCPP_ERROR(this->get_logger(), "ARM Action服务器不可用，跳过机械臂动作");
      // 继续下一个目标点
      current_goal_index_++;
      send_next_goal();
      return;
    }

    auto goal_msg = ARM::Goal();
    goal_msg.action_type = 5;
    
    RCLCPP_INFO(this->get_logger(), "发送机械臂动作 5...");

    auto send_goal_options = rclcpp_action::Client<ARM>::SendGoalOptions();
    
    // 设置结果回调
    send_goal_options.result_callback =
      [this](const GoalHandleARM::WrappedResult & result) {
        if (result.code == rclcpp_action::ResultCode::SUCCEEDED)
        {
          RCLCPP_INFO(this->get_logger(), "✓ 机械臂动作 5 完成: %s", 
                      result.result->message.c_str());
          
          // 动作5完成后，执行二维码识别type1
          RCLCPP_INFO(this->get_logger(), "等待1秒后执行二维码识别（type1）...");
          std::this_thread::sleep_for(1s);
          execute_qr_recognition_type1();
        }
        else
        {
          RCLCPP_ERROR(this->get_logger(), "✗ 机械臂动作 5 失败: %s", 
                       result.result->message.c_str());
          // 失败也继续下一个目标点
          current_goal_index_++;
          send_next_goal();
        }
      };

    // 设置反馈回调（减少日志输出）
    send_goal_options.feedback_callback =
      [this](GoalHandleARM::SharedPtr,
             const std::shared_ptr<const ARM::Feedback> feedback) {
        // RCLCPP_INFO(this->get_logger(), "动作 5 进度: %.1f%% - %s", 
        //             feedback->progress_percentage, 
        //             feedback->current_status.c_str());
      };

    arm_action_client_->async_send_goal(goal_msg, send_goal_options);
  }

  // 执行ARM动作5，完成后执行二维码识别type1，然后执行动作6
  void execute_arm_action_type5_qr_type6()
  {
    RCLCPP_INFO(this->get_logger(), "======================================");
    RCLCPP_INFO(this->get_logger(), "开始执行机械臂动作5...");
    RCLCPP_INFO(this->get_logger(), "======================================");
    
    // 等待ARM action服务器可用
    if (!arm_action_client_->wait_for_action_server(5s))
    {
      RCLCPP_ERROR(this->get_logger(), "ARM Action服务器不可用，跳过机械臂动作");
      // 继续下一个目标点
      current_goal_index_++;
      send_next_goal();
      return;
    }

    auto goal_msg = ARM::Goal();
    goal_msg.action_type = 5;
    
    RCLCPP_INFO(this->get_logger(), "发送机械臂动作 5...");

    auto send_goal_options = rclcpp_action::Client<ARM>::SendGoalOptions();
    
    // 设置结果回调
    send_goal_options.result_callback =
      [this](const GoalHandleARM::WrappedResult & result) {
        if (result.code == rclcpp_action::ResultCode::SUCCEEDED)
        {
          RCLCPP_INFO(this->get_logger(), "✓ 机械臂动作 5 完成: %s", 
                      result.result->message.c_str());
          
          // 动作5完成后，执行二维码识别type1
          RCLCPP_INFO(this->get_logger(), "等待1秒后执行二维码识别（type1）...");
          std::this_thread::sleep_for(1s);
          execute_qr_recognition_type1_then_action6();
        }
        else
        {
          RCLCPP_ERROR(this->get_logger(), "✗ 机械臂动作 5 失败: %s", 
                       result.result->message.c_str());
          // 失败也继续下一个目标点
          current_goal_index_++;
          send_next_goal();
        }
      };

    // 设置反馈回调（减少日志输出）
    send_goal_options.feedback_callback =
      [this](GoalHandleARM::SharedPtr,
             const std::shared_ptr<const ARM::Feedback> feedback) {
        // RCLCPP_INFO(this->get_logger(), "动作 5 进度: %.1f%% - %s", 
        //             feedback->progress_percentage, 
        //             feedback->current_status.c_str());
      };

    arm_action_client_->async_send_goal(goal_msg, send_goal_options);
  }

  // 执行ARM动作6，完成后执行二维码识别type1
  void execute_arm_action_type6_then_qr()
  {
    RCLCPP_INFO(this->get_logger(), "======================================");
    RCLCPP_INFO(this->get_logger(), "开始执行机械臂动作6...");
    RCLCPP_INFO(this->get_logger(), "======================================");
    
    // 等待ARM action服务器可用
    if (!arm_action_client_->wait_for_action_server(5s))
    {
      RCLCPP_ERROR(this->get_logger(), "ARM Action服务器不可用，跳过机械臂动作");
      // 继续下一个目标点
      current_goal_index_++;
      send_next_goal();
      return;
    }

    auto goal_msg = ARM::Goal();
    goal_msg.action_type = 6;
    
    RCLCPP_INFO(this->get_logger(), "发送机械臂动作 6...");

    auto send_goal_options = rclcpp_action::Client<ARM>::SendGoalOptions();
    
    // 设置结果回调
    send_goal_options.result_callback =
      [this](const GoalHandleARM::WrappedResult & result) {
        if (result.code == rclcpp_action::ResultCode::SUCCEEDED)
        {
          RCLCPP_INFO(this->get_logger(), "✓ 机械臂动作 6 完成: %s", 
                      result.result->message.c_str());
          
          // 动作6完成后，执行二维码识别type1
          RCLCPP_INFO(this->get_logger(), "等待1秒后执行二维码识别（type1）...");
          std::this_thread::sleep_for(1s);
          execute_qr_recognition_type1();
        }
        else
        {
          RCLCPP_ERROR(this->get_logger(), "✗ 机械臂动作 6 失败: %s", 
                       result.result->message.c_str());
          // 失败也继续下一个目标点
          current_goal_index_++;
          send_next_goal();
        }
      };

    // 设置反馈回调（减少日志输出）
    send_goal_options.feedback_callback =
      [this](GoalHandleARM::SharedPtr,
             const std::shared_ptr<const ARM::Feedback> feedback) {
        // RCLCPP_INFO(this->get_logger(), "动作 6 进度: %.1f%% - %s", 
        //             feedback->progress_percentage, 
        //             feedback->current_status.c_str());
      };

    arm_action_client_->async_send_goal(goal_msg, send_goal_options);
  }

  // 执行ARM动作6，完成后继续导航
  void execute_arm_action_6_then_navigate()
  {
    RCLCPP_INFO(this->get_logger(), "======================================");
    RCLCPP_INFO(this->get_logger(), "开始执行机械臂动作6...");
    RCLCPP_INFO(this->get_logger(), "======================================");
    
    // 等待ARM action服务器可用
    if (!arm_action_client_->wait_for_action_server(5s))
    {
      RCLCPP_ERROR(this->get_logger(), "ARM Action服务器不可用，跳过机械臂动作");
      // 继续下一个目标点
      current_goal_index_++;
      send_next_goal();
      return;
    }

    auto goal_msg = ARM::Goal();
    goal_msg.action_type = 6;
    
    RCLCPP_INFO(this->get_logger(), "发送机械臂动作 6...");

    auto send_goal_options = rclcpp_action::Client<ARM>::SendGoalOptions();
    
    // 设置结果回调
    send_goal_options.result_callback =
      [this](const GoalHandleARM::WrappedResult & result) {
        if (result.code == rclcpp_action::ResultCode::SUCCEEDED)
        {
          RCLCPP_INFO(this->get_logger(), "✓ 机械臂动作 6 完成: %s", 
                      result.result->message.c_str());
          
          // 动作6完成后，继续导航
          RCLCPP_INFO(this->get_logger(), "机械臂动作完成，等待3秒后继续导航...");
          current_goal_index_++;
          std::this_thread::sleep_for(3s);
          send_next_goal();
        }
        else
        {
          RCLCPP_ERROR(this->get_logger(), "✗ 机械臂动作 6 失败: %s", 
                       result.result->message.c_str());
          // 失败也继续下一个目标点
          current_goal_index_++;
          send_next_goal();
        }
      };

    // 设置反馈回调（减少日志输出）
    send_goal_options.feedback_callback =
      [this](GoalHandleARM::SharedPtr,
             const std::shared_ptr<const ARM::Feedback> feedback) {
        // RCLCPP_INFO(this->get_logger(), "动作 6 进度: %.1f%% - %s", 
        //             feedback->progress_percentage, 
        //             feedback->current_status.c_str());
      };

    arm_action_client_->async_send_goal(goal_msg, send_goal_options);
  }

  // 执行ARM动作9，完成后继续导航
  void execute_arm_action_9_then_navigate()
  {
    RCLCPP_INFO(this->get_logger(), "======================================");
    RCLCPP_INFO(this->get_logger(), "开始执行机械臂动作9...");
    RCLCPP_INFO(this->get_logger(), "======================================");
    
    // 等待ARM action服务器可用
    if (!arm_action_client_->wait_for_action_server(5s))
    {
      RCLCPP_ERROR(this->get_logger(), "ARM Action服务器不可用，跳过机械臂动作");
      // 继续下一个目标点
      current_goal_index_++;
      send_next_goal();
      return;
    }

    auto goal_msg = ARM::Goal();
    goal_msg.action_type = 9;
    
    RCLCPP_INFO(this->get_logger(), "发送机械臂动作 9...");

    auto send_goal_options = rclcpp_action::Client<ARM>::SendGoalOptions();
    
    // 设置结果回调
    send_goal_options.result_callback =
      [this](const GoalHandleARM::WrappedResult & result) {
        if (result.code == rclcpp_action::ResultCode::SUCCEEDED)
        {
          RCLCPP_INFO(this->get_logger(), "✓ 机械臂动作 9 完成: %s", 
                      result.result->message.c_str());
          
          // 动作9完成后，继续导航
          RCLCPP_INFO(this->get_logger(), "机械臂动作完成，等待3秒后继续导航...");
          current_goal_index_++;
          std::this_thread::sleep_for(3s);
          send_next_goal();
        }
        else
        {
          RCLCPP_ERROR(this->get_logger(), "✗ 机械臂动作 9 失败: %s", 
                       result.result->message.c_str());
          // 失败也继续下一个目标点
          current_goal_index_++;
          send_next_goal();
        }
      };

    // 设置反馈回调（减少日志输出）
    send_goal_options.feedback_callback =
      [this](GoalHandleARM::SharedPtr,
             const std::shared_ptr<const ARM::Feedback> feedback) {
        // RCLCPP_INFO(this->get_logger(), "动作 9 进度: %.1f%% - %s", 
        //             feedback->progress_percentage, 
        //             feedback->current_status.c_str());
      };

    arm_action_client_->async_send_goal(goal_msg, send_goal_options);
  }

  // 执行ARM动作10，完成后继续导航
  void execute_arm_action_10_then_navigate()
  {
    RCLCPP_INFO(this->get_logger(), "======================================");
    RCLCPP_INFO(this->get_logger(), "开始执行机械臂动作10...");
    RCLCPP_INFO(this->get_logger(), "======================================");
    
    // 等待ARM action服务器可用
    if (!arm_action_client_->wait_for_action_server(5s))
    {
      RCLCPP_ERROR(this->get_logger(), "ARM Action服务器不可用，跳过机械臂动作");
      // 继续下一个目标点
      current_goal_index_++;
      send_next_goal();
      return;
    }

    auto goal_msg = ARM::Goal();
    goal_msg.action_type = 10;
    
    RCLCPP_INFO(this->get_logger(), "发送机械臂动作 10...");

    auto send_goal_options = rclcpp_action::Client<ARM>::SendGoalOptions();
    
    // 设置结果回调
    send_goal_options.result_callback =
      [this](const GoalHandleARM::WrappedResult & result) {
        if (result.code == rclcpp_action::ResultCode::SUCCEEDED)
        {
          RCLCPP_INFO(this->get_logger(), "✓ 机械臂动作 10 完成: %s", 
                      result.result->message.c_str());
          
          // 动作10完成后，继续导航
          RCLCPP_INFO(this->get_logger(), "机械臂动作完成，等待3秒后继续导航...");
          current_goal_index_++;
          std::this_thread::sleep_for(3s);
          send_next_goal();
        }
        else
        {
          RCLCPP_ERROR(this->get_logger(), "✗ 机械臂动作 10 失败: %s", 
                       result.result->message.c_str());
          // 失败也继续下一个目标点
          current_goal_index_++;
          send_next_goal();
        }
      };

    // 设置反馈回调（减少日志输出）
    send_goal_options.feedback_callback =
      [this](GoalHandleARM::SharedPtr,
             const std::shared_ptr<const ARM::Feedback> feedback) {
        // RCLCPP_INFO(this->get_logger(), "动作 10 进度: %.1f%% - %s", 
        //             feedback->progress_percentage, 
        //             feedback->current_status.c_str());
      };

    arm_action_client_->async_send_goal(goal_msg, send_goal_options);
  }

  // 将二维码C区fruit_array值转换为模型3的类别序号对
  // 二维码: 1-洋葱, 2-南瓜, 3-西红柿, 4-辣椒
  // 模型3: 0,1-辣椒, 2,3-南瓜, 4,5-洋葱, 6,7-番茄
  std::vector<int32_t> convert_qr_to_model3_types(int32_t qr_fruit_type)
  {
    switch(qr_fruit_type)
    {
      case 1:  // 洋葱
        return {4, 5};
      case 2:  // 南瓜
        return {2, 3};
      case 3:  // 西红柿/番茄
        return {6, 7};
      case 4:  // 辣椒
        return {0, 1};
      default:
        RCLCPP_WARN(this->get_logger(), "未知的蔬菜类型: %d", qr_fruit_type);
        return {};
    }
  }

  // 将模型3的类别ID转换为voiceplanner的num1值
  // 模型3类别ID: 0-lajiao_ripe(成熟辣椒), 1-lajiao_unripe(未熟辣椒),
  //              2-nangua_ripe(成熟南瓜), 3-nangua_unripe(未熟南瓜),
  //              4-onion_ripe(成熟洋葱), 5-onion_unripe(未熟洋葱),
  //              6-tomato_ripe(成熟西红柿), 7-tomato_unripe(未熟西红柿)
  // voiceplanner num1: 1-成熟洋葱, 2-未熟洋葱, 3-成熟南瓜, 4-未熟南瓜,
  //                    5-成熟西红柿, 6-未熟西红柿, 7-成熟辣椒, 8-未熟辣椒
  int32_t convert_model3_to_voiceplanner(int32_t model3_class_id)
  {
    switch(model3_class_id)
    {
      case 0:  // lajiao_ripe (成熟辣椒) -> 7
        return 7;
      case 1:  // lajiao_unripe (未熟辣椒) -> 8
        return 8;
      case 2:  // nangua_ripe (成熟南瓜) -> 3
        return 3;
      case 3:  // nangua_unripe (未熟南瓜) -> 4
        return 4;
      case 4:  // onion_ripe (成熟洋葱) -> 1
        return 1;
      case 5:  // onion_unripe (未熟洋葱) -> 2
        return 2;
      case 6:  // tomato_ripe (成熟西红柿) -> 5
        return 5;
      case 7:  // tomato_unripe (未熟西红柿) -> 6
        return 6;
      default:
        RCLCPP_WARN(this->get_logger(), "未知的模型3类别ID: %d", model3_class_id);
        return 0;
    }
  }

  // 将QR识别的蔬菜类型(1-4)转换为voiceplanner的num1值
  // QR类型: 1-洋葱, 2-南瓜, 3-西红柿, 4-辣椒（都是成熟的）
  // voiceplanner num1: 1-成熟洋葱, 2-未熟洋葱, 3-成熟南瓜, 4-未熟南瓜,
  //                    5-成熟西红柿, 6-未熟西红柿, 7-成熟辣椒, 8-未熟辣椒
  int32_t convert_qr_type_to_voiceplanner(int32_t qr_type)
  {
    switch(qr_type)
    {
      case 1:  // QR洋葱 -> voiceplanner 1 (成熟洋葱)
        return 1;
      case 2:  // QR南瓜 -> voiceplanner 3 (成熟南瓜)
        return 3;
      case 3:  // QR西红柿 -> voiceplanner 5 (成熟西红柿)
        return 5;
      case 4:  // QR辣椒 -> voiceplanner 7 (成熟辣椒)
        return 7;
      default:
        RCLCPP_WARN(this->get_logger(), "未知的QR蔬菜类型: %d", qr_type);
        return 0;
    }
  }

  // 将二维码C区fruit_array值转换为voiceplanner的num1值数组
  // QR识别返回的是简单的蔬菜类型(1-4)，对应四种成熟的蔬菜
  std::vector<int32_t> convert_fruit_array_for_voiceplanner(const std::vector<int32_t>& fruit_array)
  {
    std::vector<int32_t> converted_array;
    for (int32_t qr_type : fruit_array)
    {
      int32_t voiceplanner_num = convert_qr_type_to_voiceplanner(qr_type);
      if (voiceplanner_num > 0)  // 只添加有效的转换结果
      {
        converted_array.push_back(voiceplanner_num);
      }
    }
    return converted_array;
  }

  // 执行ARM动作7，完成后继续导航
  void execute_arm_action_7_then_navigate()
  {
    RCLCPP_INFO(this->get_logger(), "======================================");
    RCLCPP_INFO(this->get_logger(), "开始执行机械臂动作7（C区数字点5,6,7,8）...");
    RCLCPP_INFO(this->get_logger(), "======================================");
    
    // 等待ARM action服务器可用
    if (!arm_action_client_->wait_for_action_server(5s))
    {
      RCLCPP_ERROR(this->get_logger(), "ARM Action服务器不可用，跳过机械臂动作");
      // 继续下一个目标点
      current_goal_index_++;
      current_number_index_++;  // 即使失败也要增加索引
      send_next_goal();
      return;
    }

    auto goal_msg = ARM::Goal();
    goal_msg.action_type = 7;
    
    // 根据当前数字目标索引传入对应的蔬菜类型参数（转换为模型3类别）
    if (current_number_index_ < qr_type2_fruit_array_.size())
    {
      int32_t qr_fruit_type = qr_type2_fruit_array_[current_number_index_];
      std::vector<int32_t> model3_types = convert_qr_to_model3_types(qr_fruit_type);
      goal_msg.right_type = model3_types;
      
      std::string types_str = "[";
      for (size_t i = 0; i < model3_types.size(); ++i)
      {
        types_str += std::to_string(model3_types[i]);
        if (i < model3_types.size() - 1) types_str += ", ";
      }
      types_str += "]";
      
      RCLCPP_INFO(this->get_logger(), "发送机械臂动作 7，第%zu个数字目标，QR类型: %d，模型3类型: %s...", 
                  current_number_index_ + 1, qr_fruit_type, types_str.c_str());
    }
    else
    {
      RCLCPP_WARN(this->get_logger(), "数字目标索引%zu超出fruit_array范围，使用空参数", 
                  current_number_index_);
      RCLCPP_INFO(this->get_logger(), "发送机械臂动作 7...");
    }

    auto send_goal_options = rclcpp_action::Client<ARM>::SendGoalOptions();
    
    // 设置结果回调
    send_goal_options.result_callback =
      [this](const GoalHandleARM::WrappedResult & result) {
        if (result.code == rclcpp_action::ResultCode::SUCCEEDED)
        {
          RCLCPP_INFO(this->get_logger(), "✓ 机械臂动作 7 完成: %s", 
                      result.result->message.c_str());
          
          // 动作7完成后，增加数字目标索引，继续导航
          current_number_index_++;
          RCLCPP_INFO(this->get_logger(), "机械臂动作完成，等待3秒后继续导航...");
          current_goal_index_++;
          std::this_thread::sleep_for(3s);
          send_next_goal();
        }
        else
        {
          RCLCPP_ERROR(this->get_logger(), "✗ 机械臂动作 7 失败: %s", 
                       result.result->message.c_str());
          // 失败也继续下一个目标点，并增加数字目标索引
          current_number_index_++;
          current_goal_index_++;
          send_next_goal();
        }
      };

    // 设置反馈回调（减少日志输出）
    send_goal_options.feedback_callback =
      [this](GoalHandleARM::SharedPtr,
             const std::shared_ptr<const ARM::Feedback> feedback) {
        // RCLCPP_INFO(this->get_logger(), "动作 7 进度: %.1f%% - %s", 
        //             feedback->progress_percentage, 
        //             feedback->current_status.c_str());
      };

    arm_action_client_->async_send_goal(goal_msg, send_goal_options);
  }

  // 执行ARM动作8，完成后继续导航
  void execute_arm_action_8_then_navigate()
  {
    RCLCPP_INFO(this->get_logger(), "======================================");
    RCLCPP_INFO(this->get_logger(), "开始执行机械臂动作8（C区数字点1,2,3,4,9,10,11,12）...");
    RCLCPP_INFO(this->get_logger(), "======================================");
    
    // 等待ARM action服务器可用
    if (!arm_action_client_->wait_for_action_server(5s))
    {
      RCLCPP_ERROR(this->get_logger(), "ARM Action服务器不可用，跳过机械臂动作");
      // 继续下一个目标点
      current_goal_index_++;
      current_number_index_++;  // 即使失败也要增加索引
      send_next_goal();
      return;
    }

    auto goal_msg = ARM::Goal();
    goal_msg.action_type = 8;
    
    // 根据当前数字目标索引传入对应的蔬菜类型参数（转换为模型3类别）
    if (current_number_index_ < qr_type2_fruit_array_.size())
    {
      int32_t qr_fruit_type = qr_type2_fruit_array_[current_number_index_];
      std::vector<int32_t> model3_types = convert_qr_to_model3_types(qr_fruit_type);
      goal_msg.right_type = model3_types;
      
      std::string types_str = "[";
      for (size_t i = 0; i < model3_types.size(); ++i)
      {
        types_str += std::to_string(model3_types[i]);
        if (i < model3_types.size() - 1) types_str += ", ";
      }
      types_str += "]";
      
      RCLCPP_INFO(this->get_logger(), "发送机械臂动作 8，第%zu个数字目标，QR类型: %d，模型3类型: %s...", 
                  current_number_index_ + 1, qr_fruit_type, types_str.c_str());
    }
    else
    {
      RCLCPP_WARN(this->get_logger(), "数字目标索引%zu超出fruit_array范围，使用空参数", 
                  current_number_index_);
      RCLCPP_INFO(this->get_logger(), "发送机械臂动作 8...");
    }

    auto send_goal_options = rclcpp_action::Client<ARM>::SendGoalOptions();
    
    // 设置结果回调
    send_goal_options.result_callback =
      [this](const GoalHandleARM::WrappedResult & result) {
        if (result.code == rclcpp_action::ResultCode::SUCCEEDED)
        {
          RCLCPP_INFO(this->get_logger(), "✓ 机械臂动作 8 完成: %s", 
                      result.result->message.c_str());
          
          // 动作8完成后，增加数字目标索引，继续导航
          current_number_index_++;
          RCLCPP_INFO(this->get_logger(), "机械臂动作完成，等待3秒后继续导航...");
          current_goal_index_++;
          std::this_thread::sleep_for(3s);
          send_next_goal();
        }
        else
        {
          RCLCPP_ERROR(this->get_logger(), "✗ 机械臂动作 8 失败: %s", 
                       result.result->message.c_str());
          // 失败也继续下一个目标点，并增加数字目标索引
          current_number_index_++;
          current_goal_index_++;
          send_next_goal();
        }
      };

    // 设置反馈回调（减少日志输出）
    send_goal_options.feedback_callback =
      [this](GoalHandleARM::SharedPtr,
             const std::shared_ptr<const ARM::Feedback> feedback) {
        // RCLCPP_INFO(this->get_logger(), "动作 8 进度: %.1f%% - %s", 
        //             feedback->progress_percentage, 
        //             feedback->current_status.c_str());
      };

    arm_action_client_->async_send_goal(goal_msg, send_goal_options);
  }

  // 执行ARM动作5，完成后执行二维码识别type2
  void execute_arm_action_type5_qr_type2()
  {
    RCLCPP_INFO(this->get_logger(), "======================================");
    RCLCPP_INFO(this->get_logger(), "开始执行机械臂动作5...");
    RCLCPP_INFO(this->get_logger(), "======================================");
    
    // 等待ARM action服务器可用
    if (!arm_action_client_->wait_for_action_server(5s))
    {
      RCLCPP_ERROR(this->get_logger(), "ARM Action服务器不可用，跳过机械臂动作");
      // 继续下一个目标点
      current_goal_index_++;
      send_next_goal();
      return;
    }

    auto goal_msg = ARM::Goal();
    goal_msg.action_type = 5;
    
    RCLCPP_INFO(this->get_logger(), "发送机械臂动作 5...");

    auto send_goal_options = rclcpp_action::Client<ARM>::SendGoalOptions();
    
    // 设置结果回调
    send_goal_options.result_callback =
      [this](const GoalHandleARM::WrappedResult & result) {
        if (result.code == rclcpp_action::ResultCode::SUCCEEDED)
        {
          RCLCPP_INFO(this->get_logger(), "✓ 机械臂动作 5 完成: %s", 
                      result.result->message.c_str());
          
          // 动作5完成后，执行二维码识别type2
          RCLCPP_INFO(this->get_logger(), "等待1秒后执行二维码识别（type2）...");
          std::this_thread::sleep_for(1s);
          execute_qr_recognition_type2();
        }
        else
        {
          RCLCPP_ERROR(this->get_logger(), "✗ 机械臂动作 5 失败: %s", 
                       result.result->message.c_str());
          // 失败也继续下一个目标点
          current_goal_index_++;
          send_next_goal();
        }
      };

    // 设置反馈回调（减少日志输出）
    send_goal_options.feedback_callback =
      [this](GoalHandleARM::SharedPtr,
             const std::shared_ptr<const ARM::Feedback> feedback) {
        // RCLCPP_INFO(this->get_logger(), "动作 5 进度: %.1f%% - %s", 
        //             feedback->progress_percentage, 
        //             feedback->current_status.c_str());
      };

    arm_action_client_->async_send_goal(goal_msg, send_goal_options);
  }

  // 执行二维码识别type1（B区），完成后执行动作6
  void execute_qr_recognition_type1_then_action6()
  {
    RCLCPP_INFO(this->get_logger(), "======================================");
    RCLCPP_INFO(this->get_logger(), "开始执行二维码识别（type1 - B区）...");
    RCLCPP_INFO(this->get_logger(), "======================================");
    
    // 等待RecognizeQR action服务器可用
    if (!qr_action_client_->wait_for_action_server(5s))
    {
      RCLCPP_ERROR(this->get_logger(), "RecognizeQR Action服务器不可用，跳过二维码识别");
      // 继续下一个目标点
      current_goal_index_++;
      send_next_goal();
      return;
    }

    auto goal_msg = RecognizeQR::Goal();
    goal_msg.type = 1;  // 使用type1进行识别
    
    RCLCPP_INFO(this->get_logger(), "发送二维码识别请求（type=1）...");

    auto send_goal_options = rclcpp_action::Client<RecognizeQR>::SendGoalOptions();
    
    // 设置结果回调
    send_goal_options.result_callback =
      [this](const GoalHandleRecognizeQR::WrappedResult & result) {
        if (result.code == rclcpp_action::ResultCode::SUCCEEDED)
        {
          RCLCPP_INFO(this->get_logger(), "✓ 二维码识别（type1）完成");
          
          // 记录返回的数据
          qr_type1_fruit_array_ = result.result->fruit_array;
          qr_type1_number_array_ = result.result->number_array;
          
          RCLCPP_INFO(this->get_logger(), "======================================");
          RCLCPP_INFO(this->get_logger(), "二维码识别结果（type1 - B区）：");
          
          // 打印fruit_array
          if (!qr_type1_fruit_array_.empty())
          {
            std::string fruit_str = "  fruit_array: [";
            for (size_t i = 0; i < qr_type1_fruit_array_.size(); ++i)
            {
              fruit_str += std::to_string(qr_type1_fruit_array_[i]);
              if (i < qr_type1_fruit_array_.size() - 1)
                fruit_str += ", ";
            }
            fruit_str += "]";
            RCLCPP_INFO(this->get_logger(), "%s", fruit_str.c_str());
          }
          else
          {
            RCLCPP_INFO(this->get_logger(), "  fruit_array: []");
          }
          
          // 打印number_array
          if (!qr_type1_number_array_.empty())
          {
            std::string number_str = "  number_array: [";
            for (size_t i = 0; i < qr_type1_number_array_.size(); ++i)
            {
              number_str += std::to_string(qr_type1_number_array_[i]);
              if (i < qr_type1_number_array_.size() - 1)
                number_str += ", ";
            }
            number_str += "]";
            RCLCPP_INFO(this->get_logger(), "%s", number_str.c_str());
          }
          else
          {
            RCLCPP_INFO(this->get_logger(), "  number_array: []");
          }
          
          RCLCPP_INFO(this->get_logger(), "======================================");
          
          // 二维码识别完成后，执行动作6
          RCLCPP_INFO(this->get_logger(), "二维码识别完成，等待1秒后执行动作6...");
          std::this_thread::sleep_for(1s);
          execute_arm_action_6_then_navigate();
        }
        else
        {
          RCLCPP_ERROR(this->get_logger(), "✗ 二维码识别（type1）失败");
          // 失败也继续下一个目标点
          current_goal_index_++;
          send_next_goal();
        }
      };

    // 设置反馈回调（减少日志输出）
    send_goal_options.feedback_callback =
      [this](GoalHandleRecognizeQR::SharedPtr,
             const std::shared_ptr<const RecognizeQR::Feedback> feedback) {
        // RCLCPP_INFO(this->get_logger(), "识别状态: %s", feedback->status.c_str());
      };

    qr_action_client_->async_send_goal(goal_msg, send_goal_options);
  }

  // 执行二维码识别type1（B区）
  void execute_qr_recognition_type1()
  {
    RCLCPP_INFO(this->get_logger(), "======================================");
    RCLCPP_INFO(this->get_logger(), "开始执行二维码识别（type1 - B区）...");
    RCLCPP_INFO(this->get_logger(), "======================================");
    
    // 等待RecognizeQR action服务器可用
    if (!qr_action_client_->wait_for_action_server(5s))
    {
      RCLCPP_ERROR(this->get_logger(), "RecognizeQR Action服务器不可用，跳过二维码识别");
      // 继续下一个目标点
      current_goal_index_++;
      send_next_goal();
      return;
    }

    auto goal_msg = RecognizeQR::Goal();
    goal_msg.type = 1;  // 使用type1进行识别
    
    RCLCPP_INFO(this->get_logger(), "发送二维码识别请求（type=1）...");

    auto send_goal_options = rclcpp_action::Client<RecognizeQR>::SendGoalOptions();
    
    // 设置结果回调
    send_goal_options.result_callback =
      [this](const GoalHandleRecognizeQR::WrappedResult & result) {
        if (result.code == rclcpp_action::ResultCode::SUCCEEDED)
        {
          RCLCPP_INFO(this->get_logger(), "✓ 二维码识别（type1）完成");
          
          // 记录返回的数据
          qr_type1_fruit_array_ = result.result->fruit_array;
          qr_type1_number_array_ = result.result->number_array;
          
          RCLCPP_INFO(this->get_logger(), "======================================");
          RCLCPP_INFO(this->get_logger(), "二维码识别结果（type1 - B区）：");
          
          // 打印fruit_array
          if (!qr_type1_fruit_array_.empty())
          {
            std::string fruit_str = "  fruit_array: [";
            for (size_t i = 0; i < qr_type1_fruit_array_.size(); ++i)
            {
              fruit_str += std::to_string(qr_type1_fruit_array_[i]);
              if (i < qr_type1_fruit_array_.size() - 1)
                fruit_str += ", ";
            }
            fruit_str += "]";
            RCLCPP_INFO(this->get_logger(), "%s", fruit_str.c_str());
          }
          else
          {
            RCLCPP_INFO(this->get_logger(), "  fruit_array: []");
          }
          
          // 打印number_array
          if (!qr_type1_number_array_.empty())
          {
            std::string number_str = "  number_array: [";
            for (size_t i = 0; i < qr_type1_number_array_.size(); ++i)
            {
              number_str += std::to_string(qr_type1_number_array_[i]);
              if (i < qr_type1_number_array_.size() - 1)
                number_str += ", ";
            }
            number_str += "]";
            RCLCPP_INFO(this->get_logger(), "%s", number_str.c_str());
          }
          else
          {
            RCLCPP_INFO(this->get_logger(), "  number_array: []");
          }
          
          RCLCPP_INFO(this->get_logger(), "======================================");
          
          // 二维码识别完成后，调用voice_planner的type=3动作，然后执行动作4
          RCLCPP_INFO(this->get_logger(), "二维码识别完成，等待2秒后调用voice_planner（type=3），然后执行动作4...");
          std::this_thread::sleep_for(2s);
          execute_voice_planner_type3_then_action4();
        }
        else
        {
          RCLCPP_ERROR(this->get_logger(), "✗ 二维码识别（type1）失败");
          // 失败也继续下一个目标点
          current_goal_index_++;
          send_next_goal();
        }
      };

    // 设置反馈回调（减少日志输出）
    send_goal_options.feedback_callback =
      [this](GoalHandleRecognizeQR::SharedPtr,
             const std::shared_ptr<const RecognizeQR::Feedback> feedback) {
        // RCLCPP_INFO(this->get_logger(), "识别状态: %s", feedback->status.c_str());
      };

    qr_action_client_->async_send_goal(goal_msg, send_goal_options);
  }

  // 执行二维码识别type2（C区）
  void execute_qr_recognition_type2()
  {
    RCLCPP_INFO(this->get_logger(), "======================================");
    RCLCPP_INFO(this->get_logger(), "开始执行二维码识别（type2 - C区）...");
    RCLCPP_INFO(this->get_logger(), "======================================");
    
    // 等待RecognizeQR action服务器可用
    if (!qr_action_client_->wait_for_action_server(5s))
    {
      RCLCPP_ERROR(this->get_logger(), "RecognizeQR Action服务器不可用，跳过二维码识别");
      // 继续下一个目标点
      current_goal_index_++;
      send_next_goal();
      return;
    }

    auto goal_msg = RecognizeQR::Goal();
    goal_msg.type = 2;  // 使用type2进行识别
    
    RCLCPP_INFO(this->get_logger(), "发送二维码识别请求（type=2）...");

    auto send_goal_options = rclcpp_action::Client<RecognizeQR>::SendGoalOptions();
    
    // 设置结果回调
    send_goal_options.result_callback =
      [this](const GoalHandleRecognizeQR::WrappedResult & result) {
        if (result.code == rclcpp_action::ResultCode::SUCCEEDED)
        {
          RCLCPP_INFO(this->get_logger(), "✓ 二维码识别（type2）完成");
          
          // 记录返回的数据
          qr_type2_fruit_array_ = result.result->fruit_array;
          qr_type2_number_array_ = result.result->number_array;
          
          RCLCPP_INFO(this->get_logger(), "======================================");
          RCLCPP_INFO(this->get_logger(), "二维码识别结果（type2 - C区）：");
          
          // 打印fruit_array
          if (!qr_type2_fruit_array_.empty())
          {
            std::string fruit_str = "  fruit_array: [";
            for (size_t i = 0; i < qr_type2_fruit_array_.size(); ++i)
            {
              fruit_str += std::to_string(qr_type2_fruit_array_[i]);
              if (i < qr_type2_fruit_array_.size() - 1)
                fruit_str += ", ";
            }
            fruit_str += "]";
            RCLCPP_INFO(this->get_logger(), "%s", fruit_str.c_str());
          }
          else
          {
            RCLCPP_INFO(this->get_logger(), "  fruit_array: []");
          }
          
          // 打印number_array
          if (!qr_type2_number_array_.empty())
          {
            std::string number_str = "  number_array: [";
            for (size_t i = 0; i < qr_type2_number_array_.size(); ++i)
            {
              number_str += std::to_string(qr_type2_number_array_[i]);
              if (i < qr_type2_number_array_.size() - 1)
                number_str += ", ";
            }
            number_str += "]";
            RCLCPP_INFO(this->get_logger(), "%s", number_str.c_str());
          }
          else
          {
            RCLCPP_INFO(this->get_logger(), "  number_array: []");
          }
          
          RCLCPP_INFO(this->get_logger(), "======================================");
          
          // 二维码识别完成后，执行voice_planner动作5，然后执行arm_control动作4
          RCLCPP_INFO(this->get_logger(), "扫码完成，等待1秒后执行voice_planner动作5和arm_control动作4...");
          std::this_thread::sleep_for(1s);
          execute_voice_planner_type5_then_action4();
        }
        else
        {
          RCLCPP_ERROR(this->get_logger(), "✗ 二维码识别（type2）失败");
          // 失败则不生成新的目标点，任务结束
          RCLCPP_INFO(this->get_logger(), "二维码识别失败，任务结束");
        }
      };

    // 设置反馈回调（减少日志输出）
    send_goal_options.feedback_callback =
      [this](GoalHandleRecognizeQR::SharedPtr,
             const std::shared_ptr<const RecognizeQR::Feedback> feedback) {
        // RCLCPP_INFO(this->get_logger(), "识别状态: %s", feedback->status.c_str());
      };

    qr_action_client_->async_send_goal(goal_msg, send_goal_options);
  }

  // 执行voice_planner动作（type=3）
  void execute_voice_planner_type3()
  {
    RCLCPP_INFO(this->get_logger(), "======================================");
    RCLCPP_INFO(this->get_logger(), "开始执行voice_planner动作（type=3）...");
    RCLCPP_INFO(this->get_logger(), "======================================");
    
    // 等待VoicePlanner action服务器可用
    if (!voice_planner_client_->wait_for_action_server(5s))
    {
      RCLCPP_ERROR(this->get_logger(), "VoicePlanner Action服务器不可用，跳过voice_planner动作");
      // 继续下一个目标点
      current_goal_index_++;
      send_next_goal();
      return;
    }

    auto goal_msg = VoicePlanner::Goal();
    goal_msg.type = 3;  // 使用type=3
    goal_msg.data = qr_type1_fruit_array_;  // 传入识别到的fruit_array
    
    // 打印将要发送的数据
    std::string data_str = "发送voice_planner请求（type=3, data=[";
    for (size_t i = 0; i < goal_msg.data.size(); ++i)
    {
      data_str += std::to_string(goal_msg.data[i]);
      if (i < goal_msg.data.size() - 1)
        data_str += ", ";
    }
    data_str += "]）...";
    RCLCPP_INFO(this->get_logger(), "%s", data_str.c_str());

    auto send_goal_options = rclcpp_action::Client<VoicePlanner>::SendGoalOptions();
    
    // 设置结果回调
    send_goal_options.result_callback =
      [this](const GoalHandleVoicePlanner::WrappedResult & result) {
        if (result.code == rclcpp_action::ResultCode::SUCCEEDED)
        {
          RCLCPP_INFO(this->get_logger(), "✓ voice_planner动作（type=3）完成: %s", 
                      result.result->message.c_str());
          
          // voice_planner完成后继续下一个目标点
          RCLCPP_INFO(this->get_logger(), "voice_planner完成，继续导航...");
          current_goal_index_++;
          std::this_thread::sleep_for(1s);
          send_next_goal();
        }
        else
        {
          RCLCPP_ERROR(this->get_logger(), "✗ voice_planner动作（type=3）失败: %s", 
                       result.result->message.c_str());
          // 失败也继续下一个目标点
          current_goal_index_++;
          send_next_goal();
        }
      };

    // 设置反馈回调
    send_goal_options.feedback_callback =
      [this](GoalHandleVoicePlanner::SharedPtr,
             const std::shared_ptr<const VoicePlanner::Feedback> feedback) {
        RCLCPP_INFO(this->get_logger(), "voice_planner状态: %s", feedback->status.c_str());
      };

    voice_planner_client_->async_send_goal(goal_msg, send_goal_options);
  }

  // 执行voice_planner动作（type=3），完成后执行动作4
  void execute_voice_planner_type3_then_action4()
  {
    RCLCPP_INFO(this->get_logger(), "======================================");
    RCLCPP_INFO(this->get_logger(), "开始执行voice_planner动作（type=3）...");
    RCLCPP_INFO(this->get_logger(), "======================================");
    
    // 等待VoicePlanner action服务器可用
    if (!voice_planner_client_->wait_for_action_server(5s))
    {
      RCLCPP_ERROR(this->get_logger(), "VoicePlanner Action服务器不可用，跳过voice_planner动作");
      // 即使失败也执行动作4
      RCLCPP_INFO(this->get_logger(), "跳过voice_planner，直接执行动作4...");
      std::this_thread::sleep_for(1s);
      execute_single_arm_action_4();
      return;
    }

    auto goal_msg = VoicePlanner::Goal();
    goal_msg.type = 3;  // 使用type=3
    goal_msg.data = qr_type1_fruit_array_;  // 传入识别到的fruit_array
    
    // 打印将要发送的数据
    std::string data_str = "发送voice_planner请求（type=3, data=[";
    for (size_t i = 0; i < goal_msg.data.size(); ++i)
    {
      data_str += std::to_string(goal_msg.data[i]);
      if (i < goal_msg.data.size() - 1)
        data_str += ", ";
    }
    data_str += "]）...";
    RCLCPP_INFO(this->get_logger(), "%s", data_str.c_str());

    auto send_goal_options = rclcpp_action::Client<VoicePlanner>::SendGoalOptions();
    
    // 设置结果回调
    send_goal_options.result_callback =
      [this](const GoalHandleVoicePlanner::WrappedResult & result) {
        if (result.code == rclcpp_action::ResultCode::SUCCEEDED)
        {
          RCLCPP_INFO(this->get_logger(), "✓ voice_planner动作（type=3）完成: %s", 
                      result.result->message.c_str());
          
          // voice_planner完成后执行动作4
          RCLCPP_INFO(this->get_logger(), "voice_planner完成，等待5秒后执行动作4...");
          std::this_thread::sleep_for(5s);
          execute_single_arm_action_4();
        }
        else
        {
          RCLCPP_ERROR(this->get_logger(), "✗ voice_planner动作（type=3）失败: %s", 
                       result.result->message.c_str());
          // 失败也执行动作4
          RCLCPP_INFO(this->get_logger(), "voice_planner失败，但等待5秒后继续执行动作4...");
          std::this_thread::sleep_for(5s);
          execute_single_arm_action_4();
        }
      };

    // 设置反馈回调
    send_goal_options.feedback_callback =
      [this](GoalHandleVoicePlanner::SharedPtr,
             const std::shared_ptr<const VoicePlanner::Feedback> feedback) {
        RCLCPP_INFO(this->get_logger(), "voice_planner状态: %s", feedback->status.c_str());
      };

    voice_planner_client_->async_send_goal(goal_msg, send_goal_options);
  }

  // 执行voice_planner动作（type=5），完成后执行动作4，然后生成C区路径
  void execute_voice_planner_type5_then_action4()
  {
    RCLCPP_INFO(this->get_logger(), "======================================");
    RCLCPP_INFO(this->get_logger(), "开始执行voice_planner动作（type=5）...");
    RCLCPP_INFO(this->get_logger(), "======================================");
    
    // 等待VoicePlanner action服务器可用
    if (!voice_planner_client_->wait_for_action_server(5s))
    {
      RCLCPP_ERROR(this->get_logger(), "VoicePlanner Action服务器不可用，跳过voice_planner动作");
      // 即使失败也执行动作4，然后生成路径
      RCLCPP_INFO(this->get_logger(), "跳过voice_planner，直接执行动作4...");
      std::this_thread::sleep_for(1s);
      execute_arm_action_4_then_generate_path();
      return;
    }

    auto goal_msg = VoicePlanner::Goal();
    goal_msg.type = 5;  // 使用type=5
    
    // voice_planner type=5需要16个参数：
    // num1~8: 蔬菜类型(1-洋葱, 2-南瓜, 3-西红柿, 4-辣椒)
    // num9~16: 数字(1-12)
    goal_msg.data = qr_type2_fruit_array_;  // 前8个：蔬菜类型(1-4)
    
    // 附加数字数组到data中
    goal_msg.data.insert(goal_msg.data.end(), 
                         qr_type2_number_array_.begin(), 
                         qr_type2_number_array_.end());
    
    // 打印发送的数据
    std::string fruit_str = "蔬菜类型(1-4): [";
    for (size_t i = 0; i < qr_type2_fruit_array_.size(); ++i)
    {
      fruit_str += std::to_string(qr_type2_fruit_array_[i]);
      if (i < qr_type2_fruit_array_.size() - 1)
        fruit_str += ", ";
    }
    fruit_str += "]";
    RCLCPP_INFO(this->get_logger(), "%s", fruit_str.c_str());
    
    std::string number_str = "数字(1-12): [";
    for (size_t i = 0; i < qr_type2_number_array_.size(); ++i)
    {
      number_str += std::to_string(qr_type2_number_array_[i]);
      if (i < qr_type2_number_array_.size() - 1)
        number_str += ", ";
    }
    number_str += "]";
    RCLCPP_INFO(this->get_logger(), "%s", number_str.c_str());
    
    std::string data_str = "发送voice_planner请求（type=5, data=[";
    for (size_t i = 0; i < goal_msg.data.size(); ++i)
    {
      data_str += std::to_string(goal_msg.data[i]);
      if (i < goal_msg.data.size() - 1)
        data_str += ", ";
    }
    data_str += "]）...";
    RCLCPP_INFO(this->get_logger(), "%s", data_str.c_str());

    auto send_goal_options = rclcpp_action::Client<VoicePlanner>::SendGoalOptions();
    
    // 设置结果回调
    send_goal_options.result_callback =
      [this](const GoalHandleVoicePlanner::WrappedResult & result) {
        if (result.code == rclcpp_action::ResultCode::SUCCEEDED)
        {
          RCLCPP_INFO(this->get_logger(), "✓ voice_planner动作（type=5）完成: %s", 
                      result.result->message.c_str());
          
          // voice_planner完成后等待10秒，然后执行动作4，然后生成路径
          RCLCPP_INFO(this->get_logger(), "voice_planner完成，等待10秒后执行动作4...");
          std::this_thread::sleep_for(10s);
          execute_arm_action_4_then_generate_path();
        }
        else
        {
          RCLCPP_ERROR(this->get_logger(), "✗ voice_planner动作（type=5）失败: %s", 
                       result.result->message.c_str());
          // 失败也执行动作4，然后生成路径
          RCLCPP_INFO(this->get_logger(), "voice_planner失败，但等待1秒后继续执行动作4...");
          std::this_thread::sleep_for(1s);
          execute_arm_action_4_then_generate_path();
        }
      };

    // 设置反馈回调
    send_goal_options.feedback_callback =
      [this](GoalHandleVoicePlanner::SharedPtr,
             const std::shared_ptr<const VoicePlanner::Feedback> feedback) {
        RCLCPP_INFO(this->get_logger(), "voice_planner状态: %s", feedback->status.c_str());
      };

    voice_planner_client_->async_send_goal(goal_msg, send_goal_options);
  }

  // 执行ARM动作4，完成后生成C区路径
  void execute_arm_action_4_then_generate_path()
  {
    RCLCPP_INFO(this->get_logger(), "======================================");
    RCLCPP_INFO(this->get_logger(), "开始执行机械臂动作4...");
    RCLCPP_INFO(this->get_logger(), "======================================");
    
    // 等待ARM action服务器可用
    if (!arm_action_client_->wait_for_action_server(5s))
    {
      RCLCPP_ERROR(this->get_logger(), "ARM Action服务器不可用，跳过机械臂动作");
      // 即使失败也生成路径
      generate_path_and_continue();
      return;
    }

    auto goal_msg = ARM::Goal();
    goal_msg.action_type = 4;
    
    RCLCPP_INFO(this->get_logger(), "发送机械臂动作 4...");

    auto send_goal_options = rclcpp_action::Client<ARM>::SendGoalOptions();
    
    // 设置结果回调
    send_goal_options.result_callback =
      [this](const GoalHandleARM::WrappedResult & result) {
        if (result.code == rclcpp_action::ResultCode::SUCCEEDED)
        {
          RCLCPP_INFO(this->get_logger(), "✓ 机械臂动作 4 完成: %s", 
                      result.result->message.c_str());
          
          // 动作4完成后，生成路径并继续导航
          RCLCPP_INFO(this->get_logger(), "机械臂动作完成，等待1秒后生成C区路径...");
          std::this_thread::sleep_for(1s);
          generate_path_and_continue();
        }
        else
        {
          RCLCPP_ERROR(this->get_logger(), "✗ 机械臂动作 4 失败: %s", 
                       result.result->message.c_str());
          // 失败也生成路径
          generate_path_and_continue();
        }
      };

    // 设置反馈回调（减少日志输出）
    send_goal_options.feedback_callback =
      [this](GoalHandleARM::SharedPtr,
             const std::shared_ptr<const ARM::Feedback> feedback) {
        // RCLCPP_INFO(this->get_logger(), "动作 4 进度: %.1f%% - %s", 
        //             feedback->progress_percentage, 
        //             feedback->current_status.c_str());
      };

    arm_action_client_->async_send_goal(goal_msg, send_goal_options);
  }

  // 生成C区路径并继续导航
  void generate_path_and_continue()
  {
    // 根据数字数组生成新的目标点序列
    if (!qr_type2_number_array_.empty())
    {
      RCLCPP_INFO(this->get_logger(), "根据识别结果生成C区导航路径...");
      generate_waypoints_from_numbers(qr_type2_number_array_);
      
      // 重置数字目标索引为0，准备开始访问数字点
      current_number_index_ = 0;
      
      // 继续执行新生成的目标点
      current_goal_index_++;
      std::this_thread::sleep_for(2s);
      send_next_goal();
    }
    else
    {
      RCLCPP_WARN(this->get_logger(), "数字数组为空，跳过C区导航");
      RCLCPP_INFO(this->get_logger(), "所有任务已完成！");
    }
  }

  // 初始执行voice_planner动作1，等待15秒后开始任务
  void execute_initial_voice_planner()
  {
    RCLCPP_INFO(this->get_logger(), "======================================");
    RCLCPP_INFO(this->get_logger(), "开始执行初始voice_planner动作（type=1）...");
    RCLCPP_INFO(this->get_logger(), "======================================");
    
    // 等待VoicePlanner action服务器可用
    if (!voice_planner_client_->wait_for_action_server(5s))
    {
      RCLCPP_WARN(this->get_logger(), "VoicePlanner Action服务器不可用，跳过初始voice_planner动作");
      // 即使失败也要开始正常任务
      RCLCPP_INFO(this->get_logger(), "等待15秒后开始正常任务...");
      std::this_thread::sleep_for(15s);
      send_next_goal();
      return;
    }

    auto goal_msg = VoicePlanner::Goal();
    goal_msg.type = 1;  // 使用type=1
    goal_msg.data = {};  // 空数据
    
    RCLCPP_INFO(this->get_logger(), "发送voice_planner请求（type=1）...");

    auto send_goal_options = rclcpp_action::Client<VoicePlanner>::SendGoalOptions();
    
    // 设置结果回调
    send_goal_options.result_callback =
      [this](const GoalHandleVoicePlanner::WrappedResult & result) {
        if (result.code == rclcpp_action::ResultCode::SUCCEEDED)
        {
          RCLCPP_INFO(this->get_logger(), "✓ 初始voice_planner动作（type=1）完成: %s", 
                      result.result->message.c_str());
        }
        else
        {
          RCLCPP_WARN(this->get_logger(), "✗ 初始voice_planner动作（type=1）失败: %s", 
                      result.result->message.c_str());
        }
        
        // 无论成功失败，等待15秒后开始正常任务
        RCLCPP_INFO(this->get_logger(), "等待15秒后开始正常任务...");
        std::this_thread::sleep_for(15s);
        send_next_goal();
      };

    // 设置反馈回调
    send_goal_options.feedback_callback =
      [this](GoalHandleVoicePlanner::SharedPtr,
             const std::shared_ptr<const VoicePlanner::Feedback> feedback) {
        RCLCPP_INFO(this->get_logger(), "voice_planner状态: %s", feedback->status.c_str());
      };

    voice_planner_client_->async_send_goal(goal_msg, send_goal_options);
  }
};

int main(int argc, char ** argv)
{
  rclcpp::init(argc, argv);
  auto node = std::make_shared<TaskPlanner>();
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}
